import ctypes
import logging
import os
import sys
import time
import socket
import logging
import math

logging.basicConfig(filename='pick.log', level=logging.DEBUG, format='%(asctime)s %(levelname)s: %(message)s')


class POSE(ctypes.Structure):
    _fields_ = [("px", ctypes.c_float),
                ("py", ctypes.c_float),
                ("pz", ctypes.c_float),
                ("rx", ctypes.c_float),
                ("ry", ctypes.c_float),
                ("rz", ctypes.c_float)]


class SockCom():
    """
    sock的建立 发送 接受
    """

    def __init__(self,ip,port):

        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.setblocking(True)  #设置阻塞
        # 连接到目标主机和端口
        server_address = (ip, port)
        logging.info('connecting to %s port %s' % server_address)
        self.tag = self.sock.connect_ex(server_address)

        self.is_connect()

    def is_connect(self):

        if self.tag: #非0显示连接失败
            logging.error(f'socket连接失败:{self.tag}')

        else:
            logging.info(f'socket连接成功')



    def send(self,character):


        """
        character:给照相机发送的指令 发送指令 paizhao就会拍照
        """

        self.message = character
        logging.info('sending "%s"' % self.message)
        self.sock.sendall(self.message.encode())

    def recv(self):
        """

        :return: material,px,py  代表当前识别出的颜色的数字，物品在相机坐标系x轴的位置，物品在相机坐标系y轴的位置

        """
        # 接收返回值
        logging.info('开始拍照')
        data = self.sock.recv(1024)
        str2 = data.decode()

        while str2 == "00":
            logging.info(f'当前相机返回的信息：00')

            self.sock.send(self.message.encode('utf-8'))
            data = self.sock.recv(1024)
            str2 = data.decode()
            time.sleep(2)

        logging.info(f'当前相机返回的信息：{str2}')

        try:
            result = str2.split(',')

            if len(result) > 1:
                material = result[0]
                px = float(result[1])/1000 - 0.26024 #吸取基准点X
                py = float(result[2])/1000 + 0.145 #吸取基准点Y

                # px = float(result[1])/1000 - 0.26
                # py = float(result[2])/1000 + 0.15



                rz = float(result[3])* math.pi /180

                return [material, px, py, rz]

            else:

                return result[0]

        except Exception as e:
            logging.error(f'拍照处理结果出错：{e}')


    def close(self):

        self.sock.close()


class Arm:
    def __init__(self, ip=''):
        self.ip = ip
        CUR_PATH = os.path.dirname(os.path.realpath(__file__))
        dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
        self.pDll = ctypes.cdll.LoadLibrary(dllPath)
        #   API 初始化
        self.pDll.RM_API_Init(65, 0)

        #   连接机械臂
        byteIP = bytes(self.ip, "gbk")
        self.nSocket = self.pDll.Arm_Socket_Start(byteIP, 8080, 2000)
        logging.info(f'连接机械臂完毕')
        self.ret = self.pDll.Arm_Socket_State(self.nSocket)
        logging.info(f'连接状态：{self.ret}')


    def load_material(self):
        """
        上料机构推出物料
        :return:
        """
        self.pDll.Set_IO_State.argtypes = (ctypes.c_int, ctypes.c_int, ctypes.c_byte,
                                           ctypes.c_bool, ctypes.c_bool)
        self.pDll.Set_IO_State.restype = ctypes.c_int
        #   推出物料
        ret = self.pDll.Set_IO_State(self.nSocket, 0, 2, 1, 1)
        time.sleep(1.5)
        logging.info(f'推出物料：{ret}')
        #   连杆收回
        ret = self.pDll.Set_IO_State(self.nSocket, 0, 2, 0, 1)
        logging.info(f'连杆收回：{ret}')

    def movej(self, joint, v, r, block):

        float_joint = ctypes.c_float * 6
        joint = float_joint(*joint)

        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte,
                                        ctypes.c_float, ctypes.c_bool)

        self.pDll.Movej_Cmd.restype = ctypes.c_int

        tag = self.pDll.Movej_Cmd(self.nSocket, joint, v, r, block)
        logging.info(f'Movej_Cmd:{tag}')
        return tag

    def movel(self, pose, v, r, block):
        po1 = POSE()
        po1.px, po1.py, po1.pz, po1.rx, po1.ry, po1.rz = pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]
        self.pDll.Movel_Cmd.argtypes = (ctypes.c_int, POSE, ctypes.c_byte, ctypes.c_float, ctypes.c_int)
        self.pDll.Movel_Cmd.restype = ctypes.c_int
        tag = self.pDll.Movel_Cmd(self.nSocket, po1, v, r, block)
        logging.info(f'Movel_Cmd:{tag}')
        return tag

    def movej_p(self, pose, v, r, block):

        tag = self.pDll.Movej_P_Cmd(self.nSocket, pose, v, r, block)  # pose 相机识别计算结果  v:20 r:0 block:1
        logging.info(f'Movej_P_Cmd执行结果:{tag}')
        return tag

    def set_io(self, io, num, state, block):

        tag = self.pDll.Set_IO_State(self.nSocket, io, num, state, block)
        logging.info(f'Set_IO_State执行的结果：{tag}')

        return tag



    def close(self):
        self.pDll.Arm_Socket_Close(self.nSocket)



def suction_back(arm, result):
    """吸取物料准备放回"""
    pose = result[1:3] + [0.34988, 3.134, -0.027, 1.052]

    ret = arm.movej_p(pose, 20, 0, 1)
    logging.info(f'移动到正上方：{ret}')

    ret = arm.movej_p(pose[:2] + [0.239039, 3.135, -0.008, 1.052], 20, 0, 1)
    logging.info(f'移动到正上方：{ret}')

    ret = arm.set_io(0, 4, 0, 1)
    logging.info(f'准备吸取：{ret}')
    ret = arm.set_io(0, 4, 1, 1)
    logging.info(f'吸取物料：{ret}')


def suction_central(arm, result):
    """吸取物料放置到中央导轨"""
    pose = result[1:3] + [0.394929, 3.135, -0.008, 1.052]
    ret = arm.movej_p(pose, 20, 0, 1)
    logging.info(f'移动到正上方：{ret}')

    ret = arm.movej_p(pose[:2] + [0.239039, 3.135, -0.008, 1.052], 20, 0, 1)
    logging.info(f'移动到正上方：{ret}')

    ret = arm.set_io(0, 4, 0, 1)
    logging.info(f'准备吸取：{ret}')
    ret = arm.set_io(0, 4, 1, 1)
    logging.info(f'吸取物料：{ret}')


def put_back(arm, result):
    """放回上料机构"""
    ret = arm.movej_p([-0.25183, 0.14271, 0.410299, -3.105, -0.004, 1.047], 20, 0, 1)
    logging.info(f'放回上料机构动作1：{ret}')

    ret = arm.movej_p([-0.260329, 0.23496, 0.39519, 3.131, -0.008, result[3] + 1.047], 20, 0, 1)
    logging.info(f'放回上料机构动作2：{ret}')

    #   松开物料
    arm.set_io(0, 4, 0, 1)
    logging.info(f'松开物料：{ret}')

    #   机械臂回到起点
    joint = [-39.433, 3.204, 87.278, -1.629, 89.719, -100.253]
    ret = arm.movej(joint, 20, 0, 1)
    logging.info(f'机械臂回到拍照位置：{ret}')


def put_central(arm, result):
    """放置到指定位置"""
    ret = arm.movej_p([-0.25183, 0.14271, 0.410299, -3.105, -0.004, 1.047], 20, 0, 1)
    logging.info(f'放置蓝色物料到指定位置动作1：{ret}')

    joint = [19.188, 12.008, 62.835, -0.006, 105.886, -41.108]
    ret = arm.movej(joint, 20, 0, 1)
    logging.info(f'放置蓝色物料到指定位置动作2：{ret}')

    ret = arm.movej_p([-0.240009, -0.083519, 0.40181, 3.133, -0.010, 1.052 + result[3]], 20, 0, 1)
    logging.info(f'放置蓝色物料到指定位置动作3：{ret}')

    ret = arm.movej_p([-0.240009, -0.083519, 0.30048, 3.132, -0.009, 1.052 + result[3]], 20, 0, 1)
    logging.info(f'放置蓝色物料到指定位置动作4：{ret}')

    #   松开物料
    ret = arm.set_io(0, 4, 0, 1)
    logging.info(f'松开物料：{ret}')

    #   机械臂回到起点
    joint = [-40.755, 3.548, 88.918, -0.009, 86.097, -100.831]
    ret = arm.movej(joint, 20, 0, 1)
    logging.info(f'机械臂回到初始位置：{ret}')


def tool_change(arm):
    """安装末端工具"""
    logging.info('视觉机械臂开始移动至工具架处')

    joint = [91.274, 25.407, 86.147, 0.903, 70.375, -61.836]
    arm.movej(joint, 20, 0, 1)

    arm.set_io(0, 3, True, 1)  # 装备工具 快换工具缩进

    time.sleep(1)

    arm.movel([0.00873, -0.300410, 0.107139, 3.106, -0.003, 2.668], 20, 0, 1)

    logging.info('视觉机械臂成功移动至工具架处')

    logging.info('视觉机械臂开始装备吸笔')

    arm.set_io(0, 3, False, 1)  # 快换工具张开

    logging.info('视觉机械臂装备吸笔完毕')

    logging.info('视觉机械臂开始转移至物料上方')

    arm.movel([0.006809, -0.296429, 0.28016, 3.118, -0.002, 2.668], 20, 0, 1)

    joint = [-39.433, 3.204, 87.278, -1.629, 89.719, -100.253]
    arm.movej(joint, 20, 0, 1)  # 拍照点

    logging.info('视觉机械臂转移至物料上方')


def main(ROT1, sock):
    tool_change(ROT1)
    ROT1.load_material()

    sock.send('paizhao')
    result = sock.recv()
    color = 3
    while result[0] != color:
        suction_back(ROT1, result)
        put_back(ROT1, result)

        ROT1.load_material()

        sock.send('paizhao')
        result = sock.recv()

    suction_central(ROT1, result)
    put_central(ROT1, result)

    ROT1.close()
    sock.close()


if __name__ == '__main__':
    sock = SockCom('192.168.1.66', 6000)
    ROT1 = Arm(ip="192.168.1.19")
    main(ROT1, sock)
